package robot;
import java.io.IOException;

import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
//import java.util.HashMap;
import java.util.Iterator;
//import java.util.Map;
import java.util.Timer;
//import java.lang.reflect.Field;

public class OnBoard {
	private static final int EMPTY_CODE = -1;
	private static final int TOO_CLOSE_TO_WALL = 500999;
	private static final int BACK_UP_COMMAND = 102005;
	final static int uploadProgram = 999999;
	static boolean isUploadProgram = false;
	final static int confirmInt = 999050;
	static Robot robot = new Robot();
	static Connection connection = new Connection();
	static SensorController sensorController = new SensorController();
	//static Map<String, String[]> statusMap = new HashMap<String, String[]>();
	private static long start;
	Timer time = new Timer();



	public static void main(String[] args) throws IOException
	{

		boolean keepItRunning = true;
		//connect to bluetooth code moved to connection class 
		connection.connect();

		int n; //holds the incoming command

		//THREAD 1
		//look for sensor data in one loop/thread
		//while (keepItRunning){
		//	checkErrors();

		//}
		//END THREAD 1


		//THREAD 2
		//look for the command
		n = 0;
		while (keepItRunning){

			long latency;
			latency = System.currentTimeMillis()-start;

			if(isUploadProgram  && latency >= 5000)
			{
				connection.sendIntToBase(confirmInt);
				isUploadProgram = false;
				start = 0;
				latency = 0;
			}

			try{
				n= connection.getNextCommand();
			}
			catch (Exception e) {
				System.exit(0); 
			}
			if (n==uploadProgram){
				isUploadProgram = true;
				start = System.currentTimeMillis();
			}
			else {
				if (n!=EMPTY_CODE){

					String ntoString = Integer.toString(n);
					if(ntoString.startsWith("11111"))
					{
						sensorController.getSensorData(ntoString);
					}
					else
					{
						LCD.drawInt(n,7,0,1);
						robot.execute(n);
						System.out.println("N : "+n);
					}
				}
			}
		}

		try {
			connection.shutdown();
		} catch (InterruptedException e) {
		} //unreachable code 

	}

	private static void checkErrors() {
		if (connection.hasWeakConnection())
		{
			System.out.println("Haz week connection!");

			robot.halt();
		}

		if(sensorController.tooClose())
		{
			System.out.println("Stopped!");
			robot.halt(); //halt
			robot.execute(BACK_UP_COMMAND); //back up 5cm.
			connection.sendIntToBase(TOO_CLOSE_TO_WALL); //ultrasonic Sensor too close
		}		
	}

	public static Robot getRobot(){
		return robot;
	}
}

